package bojan.robots.simulation;

import static src.bojan.ConfigurationParams.MAX_NEW_GOALS;
import sim.engine.SimState;
import sim.field.continuous.Continuous2D;
import sim.util.Double2D;
import src.bojan.ConfigurationParams;
import src.bojan.GridInitializer;
import src.bojan.Navigator;
import src.bojan.TerrainMap;
import src.bojan.planning.Planner;
import bojan.robots.motion.DistanceEvaluator;
import bojan.robots.motion.Driver;
import bojan.robots.motion.Motion;
import bojan.robots.motion.MotionValidator;
import bojan.robots.motion.Navigation;
import bojan.robots.motion.NavigationFactory;

public class Simulation extends SimState {

	protected Continuous2D grid = new GridInitializer(30, 30, 1.0).unknown();

	public Continuous2D realGrid = new GridInitializer(30, 30, 1.0)
			.knownSquares();
	public Motion motion = new Motion(1.0);
	public TerrainMap map = new TerrainMap(grid);
	public TerrainMap realMap = new TerrainMap(realGrid);
	public Double2D init = new Double2D(2.5, 2.5);

	protected DistanceEvaluator distanceEvaluator = new DistanceEvaluator();
	protected Driver driver = new Driver(realGrid, motion, realMap,
			distanceEvaluator);

	private MotionValidator motionValidator = new MotionValidator(grid, map);
	
	protected Planner planner = new Planner(grid, map,
			ConfigurationParams.RADIUS, ConfigurationParams.THRESHOLD,
			MAX_NEW_GOALS);

	public Simulation(long seed) {
		super(seed);
	}

	@Override
	public void start() {
		super.start();
		
		 Navigator robotA = new Navigator(grid, new Double2D(2.5, 2.5), map, realMap, driver,
				distanceEvaluator, motionValidator);
		 Navigator robotB = new Navigator(grid, new Double2D(12.5, 12.5), map, realMap, driver,
					distanceEvaluator, motionValidator);
		 Navigator robotC = new Navigator(grid, new Double2D(22.5, 22.5), map, realMap, driver,
					distanceEvaluator, motionValidator);
		
		schedule.clear();
		schedule.scheduleRepeating(NavigationFactory.create(robotA, planner, map));
		schedule.scheduleRepeating(NavigationFactory.create(robotB, planner, map));
		schedule.scheduleRepeating(NavigationFactory.create(robotC, planner, map));

	}

	public static void main(String[] args) {
		doLoop(Simulation.class, args);
		System.exit(0);
	}

}
